/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package org.electrode.base;

import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.tables.ITable;

/**
 * 
 * @author scheuerg
 */
public class ArcadeDriveControl implements DriveControl {
	private final XboxJoystick xbox;
	private ITable table;
	private double left;
	private double right;

	public ArcadeDriveControl(XboxJoystick joystick) {
		this.xbox = joystick;
	}

	public MotorChange updateMotors() {
		left = calcLeftMotor();
		right = calcRightMotor();
		return new MotorChange(left, right);
	}

	public void initTable(ITable table) {
		this.table = table;
	}

	public void updateTable() {
		if (null == table) {
			initTable(NetworkTable.getTable("Arcade"));
		}
		table.putNumber("LEFT", left);
		table.putNumber("RIGHT", right);
	}

	private double calcLeftMotor() {
		double z = xbox.getZ();
		if (z != 0.0) {
			return z;
		}
		double x = xbox.getRightX();
		double y = xbox.getLeftY();
		if (x < 0) {
			return y * (1 + x);
		} else {
			return y;
		}
	}

	private double calcRightMotor() {
		double z = xbox.getZ();
		if (z != 0.0) {
			return -z;
		}
		double x = xbox.getRightX();
		double y = xbox.getLeftY();
		if (x > 0) {
			return y * (1 - x);
		} else {
			return y;
		}
	}
}
